# -*- coding: utf-8 -*-
"""
Module for defining the wing kinematics.

This module provides functions to calculate the angular position of the wings
as a function of time, based on the prescribed sinusoidal motions for
flapping, pitching, and deviation.
"""

import numpy as np
# The 'config' module is now imported only in the testing block below.
# The WingKinematics class receives the config object via dependency injection.

class WingKinematics:
    """
    Calculates the orientation of a dragonfly wing over time.

    This class implements the sinusoidal equations of motion for a single
    wing, providing the flapping, pitching, and deviation angles for any
    given point in time.
    """
    def __init__(self, wing_config):
        """
        Initializes the WingKinematics object with parameters from the config.
        
        Args:
            wing_config: A configuration object containing kinematic parameters.
        """
        self.freq = wing_config.FLAPPING_FREQUENCY
        self.omega = 2 * np.pi * self.freq  # Angular frequency

        # Amplitudes
        self.phi_amp = wing_config.FLAPPING_AMPLITUDE / 2 # Flapping
        self.theta_amp = wing_config.PITCHING_AMPLITUDE / 2 # Pitching
        self.psi_amp = wing_config.DEVIATION_AMPLITUDE / 2 # Deviation

        # Phase lags
        self.theta_phase = wing_config.PITCHING_PHASE_LAG
        self.psi_phase = wing_config.DEVIATION_PHASE_LAG

    def get_angles(self, t: float) -> tuple[float, float, float]:
        """
        Calculates the instantaneous flapping, pitching, and deviation angles.

        Args:
            t (float): The current simulation time.

        Returns:
            A tuple containing:
            - phi (float): The flapping angle in radians.
            - theta (float): The pitching angle in radians.
            - psi (float): The deviation angle in radians.
        """
        # Flapping angle (phi)
        phi = self.phi_amp * np.sin(self.omega * t)

        # Pitching angle (theta)
        theta = self.theta_amp * np.sin(self.omega * t + self.theta_phase)

        # Deviation angle (psi)
        psi = self.psi_amp * np.sin(self.omega * t + self.psi_phase)

        return phi, theta, psi

    def get_rotation_matrix(self, t: float) -> np.ndarray:
        """
        Calculates the 3D rotation matrix for the wing at a given time.
        
        The rotation order can significantly impact the final orientation.
        A common convention is Flapping -> Deviation -> Pitching.

        Args:
            t (float): The current simulation time.

        Returns:
            np.ndarray: A 3x3 rotation matrix.
        """
        phi, theta, psi = self.get_angles(t)

        # Rotation matrix for flapping (around x-axis)
        R_flap = np.array([
            [1, 0, 0],
            [0, np.cos(phi), -np.sin(phi)],
            [0, np.sin(phi), np.cos(phi)]
        ])

        # Rotation matrix for deviation (around z-axis)
        R_dev = np.array([
            [np.cos(psi), -np.sin(psi), 0],
            [np.sin(psi), np.cos(psi), 0],
            [0, 0, 1]
        ])

        # Rotation matrix for pitching (around y-axis, the wing's spanwise axis)
        R_pitch = np.array([
            [np.cos(theta), 0, np.sin(theta)],
            [0, 1, 0],
            [-np.sin(theta), 0, np.cos(theta)]
        ])
        
        # Combine rotations. The order matters.
        # Example order: R_pitch * R_dev * R_flap
        # This means we first flap, then deviate, then pitch.
        rotation_matrix = R_pitch @ R_dev @ R_flap
        
        return rotation_matrix


if __name__ == '__main__':
    # Example Usage:
    # Plot the kinematic angles over one flapping cycle.
    import matplotlib.pyplot as plt
    import os
    import sys

    # Add project root to path to allow importing 'config' when run directly
    PROJECT_ROOT = os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))
    if PROJECT_ROOT not in sys.path:
        sys.path.append(PROJECT_ROOT)
    import config

    kinematics = WingKinematics(config)
    
    period = 1.0 / config.FLAPPING_FREQUENCY
    t_vals = np.linspace(0, period, 200)
    
    phis, thetas, psis = [], [], []
    for t in t_vals:
        phi, theta, psi = kinematics.get_angles(t)
        phis.append(np.rad2deg(phi))
        thetas.append(np.rad2deg(theta))
        psis.append(np.rad2deg(psi))

    plt.figure(figsize=(12, 6))
    plt.plot(t_vals, phis, label=r'Flapping ($\phi$)')
    plt.plot(t_vals, thetas, label='Pitching ($\\theta$)')
    plt.plot(t_vals, psis, label=r'Deviation ($\psi$)')
    plt.title('Wing Kinematic Angles over One Cycle')
    plt.xlabel('Time (s)')
    plt.ylabel('Angle (degrees)')
    plt.grid(True)
    plt.legend()
    plt.show()

    # Print the rotation matrix at t=0 and t=period/4
    R_start = kinematics.get_rotation_matrix(0)
    R_quarter = kinematics.get_rotation_matrix(period / 4)
    print("\nRotation Matrix at t=0:\n", R_start)
    print("\nRotation Matrix at t = T/4:\n", R_quarter) 